#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     armtouch,       sensorTouch)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  motorA,          harvesterL,    tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          harvesterR,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorL,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     elbow,         tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     harvesterlift, tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    leftservo,            tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_2,    rightservo,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

task main()
{
  // wait for start of tele-op phase
  int threshold = 20;             /* Int 'threshold' will allow us to ignore low       */
  /* readings that keep our robot in perpetual motion. */
  //  servoChangeRate[wrist] = 20;
  float scale = 0.5; // 0.3; //1.0; // 0.5;
#if 0
while (true)
  {

    getJoystickSettings(joystick);

    if (joy1Btn (5)== 1 ){
      scale = 0.8;
    }
    else if (joy1Btn (7)== 1 ){
      scale = 0.3;
    }
    else {
      //scale = 0.5;
    }
    if (joy1Btn(4) == 1 ) {
      servo[servo3] = 1;
      } else if (joy1Btn(6) == 1 ) {
      servo[servo3] = 127;
      } else {
      servo[servo3] = 65;
    }
  }
#endif
while (1)
{
servo [servo3]=34;
wait1Msec (500);
servo [servo3]=127;
wait1Msec (500);
servo [servo3]=0;
servo [servo4]=127;
wait1Msec (500);
servo [servo4]=0;
servo [servo5]=127;
wait1Msec (500);
servo [servo5]=0;
wait1Msec (500);
}
}
